// **************
// gyro+acc IMU
// **************
static int16_t gyroData[3] = {0,0,0};
static int16_t gyroZero[3] = {0,0,0};
static int16_t accZero[3]  = {0,0,0};
static int16_t accSmooth[3];  //projection of smoothed and normalized gravitation force vector on x/y/z axis, as measured by accelerometer


static uint16_t calibratingA;       // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
static uint16_t calibratingG;

static int16_t acc_1G = 200;       //this is the 1G measured acceleration (nunchuk)
static int16_t acc_25deg = 85;     //this is the the ACC value measured on x or y axis for a 25deg inclination (nunchuk) = acc_1G * sin(25)
static uint8_t nunchukPresent = 0;
static uint8_t accPresent = 0;     //I2C or ADC acc present


uint8_t rawIMU(uint8_t withACC) { //if the WMP or NK are oriented differently, it can be changed here
	i2c_WMP_getRawADC();
	if ( (rawADC_WMP[5]&0x02) == 0x02 && (rawADC_WMP[5]&0x01) == 0 ) {// motion plus data
		gyroADC[PITCH] =  ( ((rawADC_WMP[5]>>2)<<8) + rawADC_WMP[2] );
		gyroADC[ROLL]  = -( ((rawADC_WMP[4]>>2)<<8) + rawADC_WMP[1] );
		gyroADC[YAW]   = -( ((rawADC_WMP[3]>>2)<<8) + rawADC_WMP[0] );
		return 1;
	} else if ( (rawADC_WMP[5]&0x02) == 0 && (rawADC_WMP[5]&0x01) == 0) { //nunchuk data
		accADC[PITCH] =  ( (rawADC_WMP[3]<<2)        + ((rawADC_WMP[5]>>4)&0x2) );
		accADC[ROLL]  =  ( (rawADC_WMP[2]<<2)        + ((rawADC_WMP[5]>>3)&0x2) );
		accADC[YAW]   = -( ((rawADC_WMP[4]&0xFE)<<2) + ((rawADC_WMP[5]>>5)&0x6) );
		return 0;
	} else {
		return 2;
	}
}

uint8_t updateIMU(uint8_t withACC) {
	static int32_t g[3];
	static int32_t a[3];
	uint8_t axis;
	static int16_t previousGyroADC[3] = {0,0,0};
	uint8_t r;
	r=rawIMU(withACC);
	if (currentTime < (neutralizeTime + NEUTRALIZE_DELAY)) {//we neutralize data in case of blocking+hard reset state
		for (axis = 0; axis < 3; axis++) {gyroADC[axis]=0;accADC[axis]=0;}
		accADC[YAW] = acc_1G;
	} else {
		if (r == 1) { //gyro
			if (calibratingG>0) {
				for (axis = 0; axis < 3; axis++) {
					if (calibratingG>1) {
						if (calibratingG == 400) g[axis]=0;
						g[axis] +=gyroADC[axis];
						gyroADC[axis]=0;
					} else {
						gyroZero[axis]=(g[axis]+200)/399;
						blinkLED(10,15,1+3*nunchukPresent);
					}
				}
				calibratingG--;
			}
			gyroADC[ROLL]  = gyroADC[ROLL]  - gyroZero[ROLL];
			gyroADC[PITCH] = gyroADC[PITCH] - gyroZero[PITCH];
			gyroADC[YAW]   = gyroADC[YAW]   - gyroZero[YAW];
			gyroADC[ROLL]  = (rawADC_WMP[3]&0x01)     ? gyroADC[ROLL]/5  : gyroADC[ROLL] ;   //the ratio 1/5 is not exactly the IDG600 or ISZ650 specification 
			gyroADC[PITCH] = (rawADC_WMP[4]&0x02)>>1  ? gyroADC[PITCH]/5 : gyroADC[PITCH] ;  //we detect here the slow of fast mode WMP gyros values (see wiibrew for more details)
			gyroADC[YAW]   = (rawADC_WMP[3]&0x02)>>1  ? gyroADC[YAW]/5   : gyroADC[YAW] ;
			//anti gyro glitch, limit the variation between two consecutive readings
			for (axis = 0; axis < 3; axis++) {
				gyroADC[axis] = constrain(gyroADC[axis],previousGyroADC[axis]-100,previousGyroADC[axis]+100);
				previousGyroADC[axis] = gyroADC[axis];
			}
		}
		if (r == 0 || ( (accPresent == 1) && (withACC == 1) ) ) { //nunchuk or i2c ACC
			if (calibratingA>0) {
				if (calibratingA>1) {
					for (uint8_t axis = 0; axis < 3; axis++) {
						if (calibratingA == 400) a[axis]=0;
						a[axis] +=accADC[axis];
						accADC[axis]=0;
					}
				} else {
					accZero[ROLL]  = (a[ROLL]+200)/399;
					accZero[PITCH] = (a[PITCH]+200)/399;
					accZero[YAW]   = (a[YAW]+200)/399+acc_1G; // for nunchuk 200=1G
					//writeParams(); // write accZero in EEPROM
				}
				calibratingA--;
			} else {
				accADC[ROLL]  =    accADC[ROLL]  - accZero[ROLL] ;
				accADC[PITCH] =    accADC[PITCH] - accZero[PITCH];
				accADC[YAW]   = - (accADC[YAW]   - accZero[YAW]) ;
			}
		}
	}  
	return r;
}

void computeIMU () {
	uint8_t axis;
	static int16_t gyroADCprevious[3] = {0,0,0};
	static int16_t gyroADCp[3] = {0,0,0};
	int16_t gyroADCinter[3];
	static int16_t lastAccADC[3] = {0,0,0};
	static int16_t similarNumberAccData[3];
	static int16_t gyroDeviation[3];
	static uint32_t timeInterleave;
	static int16_t gyroYawSmooth = 0;

	//we separate the 2 situations because reading gyro values with a gyro only setup can be acchieved at a higher rate
	//gyro+nunchuk: we must wait for a quite high delay betwwen 2 reads to get both WM+ and Nunchuk data. It works with 3ms
	//gyro only: the delay to read 2 consecutive values can be reduced to only 0.65ms

	if (nunchukPresent) {
		while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
		timeInterleave=micros();
		updateIMU(0);
		getEstimatedAttitude(); // computation time must last less than one interleaving delay
		while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads    
		timeInterleave=micros();
		while(updateIMU(0) != 1) ; // For this interleaving reading, we must have a gyro update at this point (less delay)
		for (axis = 0; axis < 3; axis++) {
			// empirical, we take a weighted value of the current and the previous values
			gyroData[axis] = (gyroADC[axis]*3+gyroADCprevious[axis]+16)/4/8; // /4 is to average 4 values ; /8 is to reduce the sensibility of gyro
			gyroADCprevious[axis] = gyroADC[axis];
		}
	} else {
		updateIMU(0); //without ACC
		for (axis = 0; axis < 3; axis++)
			gyroADCp[axis] =  gyroADC[axis];
		timeInterleave=micros();
		while((micros()-timeInterleave)<650) ; //empirical, interleaving delay between 2 consecutive reads
		updateIMU(0); //without ACC
		for (axis = 0; axis < 3; axis++) {
			gyroADCinter[axis] =  gyroADC[axis]+gyroADCp[axis];
			// empirical, we take a weighted value of the current and the previous values
			gyroData[axis] = (gyroADCinter[axis]+gyroADCprevious[axis]+12)/3/8; // /3 is to average 3 values ; /8 is to reduce the sensibility of gyro
			gyroADCprevious[axis] = gyroADCinter[axis]/2;
		}
	}
}


// ************************************
// simplified IMU based on Kalman Filter
// inspired from http://starlino.com/imu_guide.html
// and http://www.starlino.com/imu_kalman_arduino.html
// for angles under 25deg, we use an approximation to speed up the angle calculation
// magnetometer addition for small angles
// ************************************
void getEstimatedAttitude(){
	uint8_t axis;  
	float R, RGyro[3];                 //R obtained from last estimated value and gyro movement;
	static float REst[3] = {0,0,1} ;   // init acc in stable mode
	static float A[2];                 //angles between projection of R on XZ/YZ plane and Z axis (in Radian)
	float wGyro = 300;               // gyro weight/smooting factor Default = 300
	float invW = 1.0/(1 + 300);
	float gyroFactor;
	static uint8_t small_angle=1;
	static uint16_t tPrevious;
	uint16_t tCurrent,deltaTime;
	float a[2], mag[2], cos_[2];

	tCurrent = micros();
	deltaTime = tCurrent-tPrevious;
	tPrevious = tCurrent;

	gyroFactor = deltaTime/200e6; //empirical, depends on WMP on IDG datasheet, tied of deg/ms sensibility

	for (axis=0;axis<2;axis++) a[axis] = gyroADC[axis]  * gyroFactor;
	for (axis=0;axis<3;axis++) accSmooth[axis] =(accSmooth[axis]*7+accADC[axis]+4)/8;

	if(accSmooth[YAW] > 0 ){ //we want to be sure we are not flying inverted  
		// a very nice trigonometric approximation: under 25deg, the error of this approximation is less than 1 deg:
		//   sin(x) =~= x =~= arcsin(x)
		//   angle_axis = arcsin(ACC_axis/ACC_1G) =~= ACC_axis/ACC_1G
		// the angle calculation is much more faster in this case
		if (accSmooth[ROLL]<acc_25deg && accSmooth[ROLL]>-acc_25deg && accSmooth[PITCH]<acc_25deg && accSmooth[PITCH]>-acc_25deg) {
			for (axis=0;axis<2;axis++) {
				A[axis] +=a[axis];
				A[axis] = ((float)accSmooth[axis]/acc_1G + A[axis]*wGyro)*invW; // =~= sin axis
			} 
			small_angle=1;
		} else {
			//magnitude vector size
			R = sqrt(square(accSmooth[ROLL]) + square(accSmooth[PITCH]) + square(accSmooth[YAW]));
			for (axis=0;axis<2;axis++) {
				if ( acc_1G*3/5 < R && R < acc_1G*7/5 && small_angle == 0 ) //if accel magnitude >1.4G or <0.6G => we neutralize the effect of accelerometers in the angle estimation
					A[axis] = atan2(REst[axis],REst[YAW]);
				A[axis] +=a[axis];
				cos_[axis] = cos(A[axis]);
				RGyro[axis]  = sin(A[axis])  / sqrt( 1.0 + square(cos_[axis])  * square(tan(A[1-axis]))); //reverse calculation of RwGyro from Awz angles
			}
			RGyro[YAW] = sqrt(abs(1.0 - square(RGyro[ROLL]) - square(RGyro[PITCH])));
			for (axis=0;axis<3;axis++)
				REst[axis] = (accADC[axis]/R + wGyro* RGyro[axis])  * invW; //combine Accelerometer and gyro readings
			small_angle=0;
		}
	}
	for (axis=0;axis<2;axis++) angle[axis] = A[axis]*572.9577951; //angle in multiple of 0.1 degree
}


void blinkLED(uint8_t num, uint8_t wait,uint8_t repeat) {
	uint8_t i,r;
	for (r=0;r<repeat;r++) {
		for(i=0;i<num;i++) {
			LEDPIN_SWITCH //switch LEDPIN state
				delay(wait);
		}
		delay(60);
	}
}